﻿// Author: Hristo Hristov
// Revision 5

using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;
//using OpenNI;
using NOVA;
using NOVA.Physics;
using NOVA.Scenery;
using BEPUphysics.MathExtensions;
using System.Globalization;

namespace NOVA.Utilities
{
    /// <summary>
    /// A set of static helper methods for the NOVA framework.
    /// </summary>
    public static class Helpers
    {
        /// <summary>
        /// Converts a given matrix from BEPU to XNA type
        /// </summary>
        public static Matrix ToXnaMatrix(Matrix3X3 mat)
            {
                Matrix matrix = Matrix.Identity;

                matrix.M11 = mat.M11; matrix.M12 = mat.M12; matrix.M13 = mat.M13;
                matrix.M21 = mat.M21; matrix.M22 = mat.M22; matrix.M23 = mat.M23;
                matrix.M31 = mat.M31; matrix.M32 = mat.M32; matrix.M33 = mat.M33;

                return matrix;
            }

        /// <summary>
        /// Converts a given matrix from XNA to BEPU type
        /// </summary>
        public static Matrix3X3 ToBEPUMatrix(Matrix mat)
        {
            Matrix3X3 matrix = new Matrix3X3();

            matrix.M11 = mat.M11; matrix.M12 = mat.M12; matrix.M13 = mat.M13;
            matrix.M21 = mat.M21; matrix.M22 = mat.M22; matrix.M23 = mat.M23;
            matrix.M31 = mat.M31; matrix.M32 = mat.M32; matrix.M33 = mat.M33;

            return matrix;
        }
        
        /// <summary>
        /// Returns Euler angles that point from one point to another
        /// </summary>
        public static Vector3 AngleTo(Vector3 from, Vector3 location)
        {
            Vector3 angle = new Vector3();
            Vector3 v3 = Vector3.Normalize(location - from);

            angle.X = (float)Math.Asin(v3.Y);
            angle.Y = (float)Math.Atan2((double)-v3.X, (double)-v3.Z);

            return angle;
        }
                
        /// <summary>
        /// Converts a Quaternion to Euler angles (X = Yaw, Y = Pitch, Z = Roll)
        /// </summary>
        public static Vector3 QuaternionToEulerAngleVector3(Quaternion rotation)
        {
            Vector3 rotationaxes = new Vector3();
            Vector3 forward = Vector3.Transform(Vector3.Forward, rotation);
            Vector3 up = Vector3.Transform(Vector3.Up, rotation);

            rotationaxes = AngleTo(new Vector3(), forward);

            if (rotationaxes.X == MathHelper.PiOver2)
            {
                rotationaxes.Y = (float)Math.Atan2((double)up.X, (double)up.Z);
                rotationaxes.Z = 0;
            }
            else if (rotationaxes.X == -MathHelper.PiOver2)
            {
                rotationaxes.Y = (float)Math.Atan2((double)-up.X, (double)-up.Z);
                rotationaxes.Z = 0;
            }
            else
            {
                up = Vector3.Transform(up, Matrix.CreateRotationY(-rotationaxes.Y));
                up = Vector3.Transform(up, Matrix.CreateRotationX(-rotationaxes.X));

                rotationaxes.Z = (float)Math.Atan2((double)-up.Z, (double)up.Y);
            }

            return rotationaxes;
        }

        /// <summary>
        /// Converts a Rotation Matrix to a quaternion, then into a Vector3 containing
        /// Euler angles (X: Pitch, Y: Yaw, Z: Roll)
        /// </summary>
        /// <param name="Rotation"></param>
        /// <returns></returns>
        public static Vector3 MatrixToEulerAngleVector3(Matrix Rotation)
        {
            Vector3 translation, scale;
            Quaternion rotation;

            Rotation.Decompose(out scale, out rotation, out translation);

            Vector3 eulerVec = QuaternionToEulerAngleVector3(rotation);

            return eulerVec;
        }

        /// <summary>
        /// Unprojects the given screen point and returns the point of intersection
        /// with the given plane in world space.
        /// </summary>
        public static Vector3 Unproject(Vector2 point, Plane plane)
        {
            Vector3 ret = Vector3.Zero;

            int iX = (int)(point.X * Core.Width);
            int iY = (int)(point.Y * Core.Height);

            // Pay attention to the current camera view
            Matrix viewMatrix = /*Scene.Camera.WorldTransformation */ Core.ViewMatrix;

            // Convert the position vector from screen to world space -> 2D to 3D conversion
            Vector3 vec1 = Core.Device.Viewport.Unproject(new Vector3(iX, iY, 0), Core.ProjectionMatrix, viewMatrix, Matrix.Identity);
            Vector3 vec2 = Core.Device.Viewport.Unproject(new Vector3(iX, iY, 1), Core.ProjectionMatrix, viewMatrix, Matrix.Identity);

            // Use the depth to scale the position
            Vector3 dir = vec2 - vec1;
            dir.Normalize();
            Ray ray = new Ray(vec1, dir);
            float? position = ray.Intersects(plane);

            if (position.HasValue)
            {
                ret = ray.Position + ray.Direction * position.Value;
            }

            return ret;
        }

        /// <summary>
        /// Creates a string representation of a Vector3.
        /// </summary>
        public static String Vector2ToString(Vector2 vec)
        {
            String str;

            str = string.Format("{0}, {1}", vec.X.ToString(NumberFormatInfo.InvariantInfo), vec.Y.ToString(NumberFormatInfo.InvariantInfo));

            return str;
        }

        /// <summary>
        /// Creates a string representation of a Vector3.
        /// </summary>
        public static String Vector3ToString(Vector3 vec)
        {
            String str;

            str = string.Format("{0}, {1}, {2}", vec.X.ToString(NumberFormatInfo.InvariantInfo), vec.Y.ToString(NumberFormatInfo.InvariantInfo), vec.Z.ToString(NumberFormatInfo.InvariantInfo));

            return str;
        }

        /// <summary>
        /// Creates a string representation of a Vector4.
        /// </summary>
        public static String Vector4ToString(Vector4 vec)
        {
            String str;

            str = string.Format("{0}, {1}, {2}, {3}", vec.X.ToString(NumberFormatInfo.InvariantInfo), vec.Y.ToString(NumberFormatInfo.InvariantInfo), vec.Z.ToString(NumberFormatInfo.InvariantInfo), vec.W.ToString(NumberFormatInfo.InvariantInfo));

            return str;
        }

        /// <summary>
        /// Creates a string representation of a Quaternion.
        /// </summary>
        public static String QuadToString(Quaternion quad)
        {
            String str;

            str = string.Format("{0}, {1}, {2}, {3}", quad.X.ToString(NumberFormatInfo.InvariantInfo), quad.Y.ToString(NumberFormatInfo.InvariantInfo), quad.Z.ToString(NumberFormatInfo.InvariantInfo), quad.W.ToString(NumberFormatInfo.InvariantInfo));

            return str;
        }

        /// <summary>
        /// Convert the given string to a vector2 by converting all
        /// numbers to float.
        /// </summary>
        public static Vector2 StringToVector2(String str)
        {
            Vector2 vec2 = Vector2.Zero;

            String[] vals = str.Split(':', ' ', '}');

            // If the string contains vector3 data ...
            if (vals[0] == "{X")
            {
                vec2.X = float.Parse(vals[1], CultureInfo.InvariantCulture);
                vec2.Y = float.Parse(vals[3], CultureInfo.InvariantCulture);
            }
            else // If the string contains shape data
            {
                // Trim the commas at the end
                vals[0] = vals[0].TrimEnd(',');
                vals[1] = vals[1].TrimEnd(',');

                vec2.X = float.Parse(vals[0], CultureInfo.InvariantCulture);
                vec2.Y = float.Parse(vals[1], CultureInfo.InvariantCulture);
            }

            return vec2;
        }

        /// <summary>
        /// Convert the given string to a vector3 by converting all
        /// numbers to float.
        /// </summary>
        public static Vector3 StringToVector3(String str)
        {
            Vector3 vec3 = Vector3.Zero;

            String[] vals = str.Split(':', ' ', '}');

            // If the string contains vector3 data ...
            if (vals[0] == "{X")
            {
                vec3.X = float.Parse(vals[1], CultureInfo.InvariantCulture);
                vec3.Y = float.Parse(vals[3], CultureInfo.InvariantCulture);
                vec3.Z = float.Parse(vals[5], CultureInfo.InvariantCulture);
            }
            else // If the string contains shape data
            {
                // Trim the commas at the end
                vals[0] = vals[0].TrimEnd(',');
                vals[1] = vals[1].TrimEnd(',');

                vec3.X = float.Parse(vals[0], CultureInfo.InvariantCulture);
                vec3.Y = float.Parse(vals[1], CultureInfo.InvariantCulture);
                vec3.Z = float.Parse(vals[2], CultureInfo.InvariantCulture);
            }

            return vec3;
        }

        /// <summary>
        /// Convert the given string to a vector4 by converting all
        /// numbers to float.
        /// </summary>
        public static Vector4 StringToVector4(String str)
        {
            Vector4 vec4 = Vector4.Zero;

            String[] vals = str.Split(':', ' ', '}');

            // If the string contains vector3 data ...
            if (vals[0] == "{X")
            {
                vec4.X = float.Parse(vals[1], CultureInfo.InvariantCulture);
                vec4.Y = float.Parse(vals[3], CultureInfo.InvariantCulture);
                vec4.Z = float.Parse(vals[5], CultureInfo.InvariantCulture);
                vec4.W = float.Parse(vals[7], CultureInfo.InvariantCulture);
            }
            else // If the string contains shape data
            {
                // Trim the commas at the end
                vals[0] = vals[0].TrimEnd(',');
                vals[1] = vals[1].TrimEnd(',');
                vals[2] = vals[2].TrimEnd(',');

                vec4.X = float.Parse(vals[0], CultureInfo.InvariantCulture);
                vec4.Y = float.Parse(vals[1], CultureInfo.InvariantCulture);
                vec4.Z = float.Parse(vals[2], CultureInfo.InvariantCulture);
                vec4.W = float.Parse(vals[2], CultureInfo.InvariantCulture);
            }

            return vec4;
        }

        /// <summary>
        /// Convert the given string to a quaternion by converting all
        /// numbers to float.
        /// </summary>
        public static Quaternion StringToQuad(String str)
        {
            Quaternion quad = Quaternion.Identity;

            String[] vals = str.Split(':', ' ', '}');

            // If the string contains vector3 data ...
            if (vals[0] == "{X")
            {
                quad.X = float.Parse(vals[1], CultureInfo.InvariantCulture);
                quad.Y = float.Parse(vals[3], CultureInfo.InvariantCulture);
                quad.Z = float.Parse(vals[5], CultureInfo.InvariantCulture);
                quad.W = float.Parse(vals[7], CultureInfo.InvariantCulture);
            }
            else // If the string contains shape data
            {
                // Trim the commas at the end
                vals[0] = vals[0].TrimEnd(',');
                vals[1] = vals[1].TrimEnd(',');
                vals[2] = vals[2].TrimEnd(',');

                quad.X = float.Parse(vals[0], CultureInfo.InvariantCulture);
                quad.Y = float.Parse(vals[1], CultureInfo.InvariantCulture);
                quad.Z = float.Parse(vals[2], CultureInfo.InvariantCulture);
                quad.W = float.Parse(vals[3], CultureInfo.InvariantCulture);
            }

            return quad;
        }
    }
}
